#include "bot.h"
#include "obstacle.h"
Bot::Bot(World* w, Path* p, Graph *g):MovingObject()
{
    firerate=0;
    path=p;
    m_stop=false;
    m_world=w;
    m_velocity=QVector2D(1.0,0);
    m_heading=QVector2D(1,0);
    nodePos=g->getNode(rand()%1000);
    this->setPos(nodePos->getPos());



    nextNodeIndex=0;


    m_astarSearch=new AStar(g,new ZeroHeuristic());
    m_astarSearch->setSource(nodePos->getIndex());

    index=nodePos->getIndex();

    m_astarSearch->setTarget(330);
    m_astarSearch->SearchPath();
    m_astarSearch->ConstructPath();

    path->newPath(m_astarSearch->newPath());

    setTargetPos(QVector2D(path->getNodePos(0).x(),path->getNodePos(0).y()));

}

Bot::~Bot()
{
   // delete m_world;
}


void Bot::paint(QPainter *painter, const QStyleOptionGraphicsItem*, QWidget*)
{

    painter->setPen(QPen(Qt::black));
    painter->setBrush(Qt::blue);
    painter->drawEllipse(0,-10,20,20);
    painter->setPen(QPen(Qt::blue));
}

inline const World* Bot::world()const
{
    return m_world;
}



void Bot::seek(const QVector2D& target)
{
    //rotateToTarget(target);
     QVector2D toTarget = (target-QVector2D(this->scenePos())).normalized();
    m_velocity+=toTarget;
    m_velocity.normalize();
}

void Bot::flee(const QVector2D& target)
{
    //rotateToTarget(targetPos);
    QVector2D toTarget = (target-QVector2D(this->scenePos())).normalized();
    m_velocity+=toTarget;
    m_velocity.normalize();
}

void Bot::arrive (const QVector2D& targetPos)
{
    targetPos.length();
}


void Bot::ObstacleAvoidance()
{
    QPointF tmp;
    QVector2D vp=QVector2D(0,0);
    float angle=0;
    float alpha=0;
    float theta=0;
    foreach(MapElement *el, m_world->m_mapelements)
    {
        int sX=static_cast<Obstacle*>(el)->getS().x();
        int sY=static_cast<Obstacle*>(el)->getS().y();

        if( (sX-this->scenePos().x())*(sX-this->scenePos().x())+(sY-this->scenePos().y())*(sY-this->scenePos().y())<200*200 )
        {

            tmp=QPointF(this->scenePos().x()+m_velocity.x()*m_faja.x(),this->scenePos().y()+m_velocity.y()*m_faja.y());
            if((tmp.x()-sX)*(tmp.x()-sX)+(tmp.y()-sY)*(tmp.y()-sY) <
                    (static_cast<Obstacle*>(el)->getR()+10)*(static_cast<Obstacle*>(el)->getR()+10))
            {




                alpha= atan(m_velocity.y()/m_velocity.x())*180/M_PI;
                theta= atan(abs(sY-tmp.y())/abs(sX-tmp.x()))*180/M_PI;

                while(alpha<0) alpha+=360;
                while(alpha>=360) alpha-=360;
                while(theta<0) theta+=360;
                while(theta>=360) theta-=360;

                if (theta>=0 && theta<45)
                {
                    if(alpha>=270 && alpha<360)
                    {
                       vp=rotate(m_velocity,-M_PI/2);
                       angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }
                else if (theta>=45 && theta<90)
                {
                    if(alpha>=0 && alpha<90)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }
                else if (theta>=90 && theta<135)
                {
                    if(alpha>=0 && alpha<90)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }
                else if (theta>=135 && theta<180)
                {
                    if(alpha>=90 && alpha<180)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }

                else if(theta>=180 && theta<225)
                {
                    if(alpha>=90 && alpha<180)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }
                else if (theta>=225 && theta<270)
                {
                    if(alpha>=180 && alpha<270)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }
                else if (theta>=270 && theta<315)
                {
                    if(alpha>=180 && alpha<270)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }
                else if (theta>=315 && theta<360)
                {
                    if(alpha>=270 && alpha<360)
                    {
                        vp=rotate(m_velocity,-M_PI/2);
                        angle=-90;
                    }
                    else
                    {
                        vp=rotate(m_velocity,M_PI/2);
                        angle=90;
                    }
                }


                /*std::cout<<"vp: ("<<vp.x()<<","<<vp.y()<<")"<<std::endl;
                */
                m_velocity+=vp;
                m_velocity.normalize();
                this->setRotation(rotation()+angle/2);
            }
        }
    }
}

QPointF Bot::getOldBotPos()
{
    return m_oldBotPos;
}

void Bot::step()
{


   double eps=10.001;


     if(!m_stop)
     {
        seek(m_targetPos);
        float len=(m_targetPos.x()-this->scenePos().x())*(m_targetPos.x()-this->scenePos().x());
        len+=(m_targetPos.y()-this->scenePos().y())*(m_targetPos.y()-this->scenePos().y());


       if(nextNodeIndex<path->m_patch.size()-1 && len <eps )
       {
           ++nextNodeIndex;
           setTargetPos(QVector2D(path->getNodePos(nextNodeIndex).x(),path->getNodePos(nextNodeIndex).y()));
           index=path->m_patch[nextNodeIndex]->getFrom()->getIndex();

       }if(nextNodeIndex>=path->m_patch.size()-1)
       {
           int aaa=rand()%1050;
           index=path->m_patch[nextNodeIndex]->getFrom()->getIndex();

           while(aaa==index)
           {
                aaa=rand()%1050;
           }

           m_astarSearch->setSource(index);
           m_astarSearch->setTarget(aaa);
           m_astarSearch->SearchPath();
           m_astarSearch->ConstructPath();
           path->newPath(m_astarSearch->newPath());
           nextNodeIndex=0;


       }
      // else if(i==0)setTargetPos(QVector2D(path->getNodePos(i).x(),path->getNodePos(i).y()));


        //flee(m_targetPos);
        //ObstacleAvoidance();
       // m_velocity.normalize();
       /* if(this->scenePos().x()>800)
        {
            this->setPos(0,this->scenePos().y());
        }else if(this->scenePos().y()>600)
        {
            this->setPos(this->scenePos().x(),0);
        }
        if(this->scenePos().x()<0)
        {
            this->setPos(780,this->scenePos().y());
        }else if(this->scenePos().y()<0)
        {
            this->setPos(this->scenePos().x(),580);
        }
        else
        {*/
            this->setPos(this->scenePos().x()+m_velocity.toPoint().x()*2,this->scenePos().y()+m_velocity.toPoint().y()*2);
        //}
    }
     fire();

}

void Bot::fire()
{
    if(firerate==0)
    {
        firerate=15;
        Bullet* bullet= new Bullet;
        bullet=new Bullet();
        bullet->setBoundingRect(QRectF(0,0,5,5));
        bullet->setPos(this->scenePos());

        double angle = acos((abs(this->scenePos().y()-master->scenePos().y()))/sqrt(
                                (this->scenePos().x()-master->scenePos().x())*(this->scenePos().x()-master->scenePos().x())+
                                (this->scenePos().y()-master->scenePos().y())*(this->scenePos().y()-master->scenePos().y())));


        if(this->scenePos().x()<master->scenePos().x() && this->scenePos().y()>=master->scenePos().y())
        {
           // angle*=-1;
            angle+=M_PI;
        }else if(this->scenePos().x()>master->scenePos().x() && this->scenePos().y()<master->scenePos().y())
        {
            angle*=1;//
        }else if(this->scenePos().x()<master->scenePos().x() && this->scenePos().y()<master->scenePos().y())
        {
            angle*=-1;//
        }else if(this->scenePos().x()>=master->scenePos().x() && this->scenePos().y()>master->scenePos().y())
        {
            angle*=-1;
            angle+=M_PI;
        }



        bullet->rotate(angle*180/M_PI);
        m_world->bullets.enqueue(bullet);
    }else
        firerate--;

}


void Bot::setTargetPos(QVector2D p)
{
    m_targetPos=p;
}

void Bot::stop()
{
    m_stop=true;
}

void Bot::start()
{
    m_stop=false;
}


void Bot::setBoundingRect(QRectF rect)
{
    m_boundingRect=rect;
    m_faja=QPointF(rect.width(),rect.height()/2);
}


ZombieMaster::ZombieMaster(World* w, Path* p, Graph *g, Bot* b)
{

    bot=b;
    m_velocity=QVector2D(1.0,0);
    m_heading=QVector2D(1,0);
    path=p;
    m_world=w;
    m_velocity=QVector2D(1.0,0);
    m_heading=QVector2D(1,0);
    nodePos=g->getNode(rand()%1000);
    this->setPos(nodePos->getPos());
    nextNodeIndex=0;
    m_astarSearch=new AStar(g,new Euclides );//new ZeroHeuristic());
    m_astarSearch->setSource(nodePos->getIndex());
    index=nodePos->getIndex();
    m_astarSearch->setTarget(b->index);
    m_astarSearch->SearchPath();
    m_astarSearch->ConstructPath();
    path->newPath(m_astarSearch->newPath());
    setTargetPos(QVector2D(path->getNodePos(0).x(),path->getNodePos(0).y()));

}

void ZombieMaster::paint(QPainter *painter, const QStyleOptionGraphicsItem*, QWidget*)
{

    painter->setPen(QPen(Qt::black));
    painter->setBrush(Qt::red);
    painter->drawEllipse(0,-11,22,22);
}

void ZombieMaster::report(Zombie* z)
{
    zombies.push_back(z);
}

void ZombieMaster::attack()
{
    foreach(Zombie* z,zombies)
    {

        z->setBot(bot);
        z->attack();

    }
}


void ZombieMaster::step()
{




    if(zombies.size()>7 && index!=bot->index)
    {


        m_astarSearch->setSource(index);

        m_astarSearch->setTarget(bot->index);
        m_astarSearch->SearchPath();
        m_astarSearch->ConstructPath();

        path->newPath(m_astarSearch->newPath());
        nextNodeIndex=0;
        setTargetPos(QVector2D(path->getNodePos(0).x(),path->getNodePos(0).y()));
        index=path->m_patch[0]->getFrom()->getIndex();


        seek(m_targetPos);


        double eps=10.00001;


        float len=(m_targetPos.x()-this->scenePos().x())*(m_targetPos.x()-this->scenePos().x());
        len+=(m_targetPos.y()-this->scenePos().y())*(m_targetPos.y()-this->scenePos().y());


        if(nextNodeIndex<path->m_patch.size()-2 && len <eps )
        {
            ++nextNodeIndex;
            setTargetPos(QVector2D(path->getNodePos(nextNodeIndex).x(),path->getNodePos(nextNodeIndex).y()));
            index=path->m_patch[nextNodeIndex]->getFrom()->getIndex();
        }



        this->setPos(this->scenePos().x()+m_velocity.toPoint().x()*2,this->scenePos().y()+m_velocity.toPoint().y()*2);
        attack();



    }

}


void ZombieMaster::setTargetPos(QVector2D p)
{
    m_targetPos=p;
}




